import threading
import time
import socket
#import pygame
import cv
import Image
import io
import serial
import Queue
import StringIO
import multiprocessing

class Serial(threading.Thread):
    def __init__(self, comport, baud, ErrorQueue):
        threading.Thread.__init__(self)
        self.ComPort=comport
        self.Baud=baud
        self.Die=False
        self.ErrorQueue=ErrorQueue
        self.Queue=Queue.Queue()
        
    def connect2serial(self):
        while not self.Die:
            print "open COM%d"%(self.ComPort+1)
            try:
                self.Serial=serial.Serial(self.ComPort, self.Baud, timeout=1)
                break
            except:
                self.ErrorQueue.put("ERROR: Connecting to COM%d"%(self.ComPort+1))
                time.sleep(1)
                
    class Write(threading.Thread):
        def __init__(self, Serial, ErrorQueue):
            threading.Thread.__init__(self)
            self.ErrorQueue=ErrorQueue
            self.Queue=Queue.Queue()
            self.Die=False
            self.Serial=Serial
        def run(self):
            while not self.Die:
                data=self.Queue.get()
                try:
                    pass
                    for ch in data:
                        print ord(ch),
                    print ""
                    self.Serial.write(chr(255) + data + chr(0))
                except:
                    if not self.ErrorQueue.full():
                        self.ErrorQueue.put("ERROR: Connecting to COM%d"%(self.ComPort+1))
                    break

    class Read(threading.Thread):
        def __init__(self, Serial, ErrorQueue):
            threading.Thread.__init__(self)
            self.ErrorQueue=ErrorQueue
            self.Queue=Queue.Queue()
            self.Die=False
            self.Serial=Serial

        def SerialDecode(self, data):
            temp=""
            for i in range(len(data)):
                temp+="%d:"%(ord(data[i]))
            return temp
                    
        def run(self):
            while not self.Die:
                try:
                    temp=self.Serial.read(1)
                    temp+=self.Serial.read(self.Serial.inWaiting())
                    temp=chr(255)+chr(1)+chr(100)+chr(255)+chr(2)+chr(110)

                except:
                    if not self.ErrorQueue.full():
                        self.ErrorQueue.put("ERROR: Connecting to COM%d"%(self.ComPort+1))
                    break
                if temp and self.Queue.empty():
                    self.Queue.put(self.SerialDecode(temp))
                else:
                    if not self.ErrorQueue.full():
                        self.ErrorQueue.put("ERROR: didn't receive any Serial data from Raupe!")
        
    def run(self):

        self.connect2serial()
        self.Write=Serial.Write(self.Serial, self.ErrorQueue)
        self.Read=Serial.Read(self.Serial, self.ErrorQueue)
        self.Write.start()
        self.Read.start()
        
        while not self.Die:
            if self.Write.isAlive() and self.Read.isAlive():
                time.sleep(1)
            else:
                self.connect2serial()
                self.Write=Serial.Write(self.Serial, self.ErrorQueue)
                self.Read=Serial.Read(self.Serial, self.ErrorQueue)
                self.Write.start()
                self.Read.start()
        self.Write.Die=True
        self.Read.Die=True
                

class ControllReception(threading.Thread):
    def __init__(self, ip, port, timeout, ErrorQueue):
        threading.Thread.__init__(self)
        self.Ip=ip
        self.Port=port
        self.Timeout=timeout
        self.ErrorQueue=ErrorQueue
        self.Die=False
        self.Serial=Serial(0, 9600, self.ErrorQueue)

    def OpenSocket(self):
        self.Socket=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.Socket.bind((self.Ip, self.Port))            # bind it to the server port
        self.Socket.listen(1)

    def WaitForClient(self):
        print "Warte auf Verbindung"
        try:
            self.Connection, self.ClientAddress = self.Socket.accept()
            print "Verbunden mit %s"%str(self.ClientAddress[0])
        except:
            self.ErrorQueue.put("ERROR: Commands->WaitForClient()")

    def ReceiveCommands(self):
        while not self.Die:
            self.data = self.Connection.recv(1024)
            if not self.data:
                self.WaitForClient()
                continue
            if self.Serial.Write.Queue.empty():
                self.Serial.Write.Queue.put(self.data)
            while not self.ErrorQueue.empty():
                self.Connection.send(self.ErrorQueue.get())
            while not self.Serial.Read.Queue.empty():
                self.Connection.send("SERIAL:"+self.Serial.Read.Queue.get())
                
    def run(self):
        self.Serial.start()
        self.OpenSocket()
        self.WaitForClient()
        self.ReceiveCommands()
        
class Capture(object):
    def __init__(self, ErrorQueue):
        object.__init__(self)
        self.Die=False
        self.CAMResolution=(320,240)
        self.picMode="L"
        self.picQuality=20
        self.data=""
        self.FPS=25
        self.ErrorQueue=ErrorQueue
        self.FrameQueue=Queue.Queue()
        self.capture = cv.CaptureFromCAM(0)
        cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_WIDTH, self.CAMResolution[0])
        cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_HEIGHT, self.CAMResolution[1])
        
    def cvFrame2jpeg(self, cvframe, resolution, mode, quality):
        im=Image.fromstring("RGB", cv.GetSize(cvframe), cvframe.tostring())
        output=StringIO.StringIO()
        if self.picMode == "L":
            im=im.convert(self.picMode)
        im.save(output, "jpeg", quality=quality)
        output.seek(0)
        jpeg=output.getvalue()
        output.close()
        return jpeg
    
    def jpeg2cvFrame(self, jpeg, resolution, mode):
        input=StringIO.StringIO()
        input.write(jpeg)
        input.seek(0)
        im=Image.open(input)
        cvframe = cv.CreateImage(resolution, mode, self.picMode)
        cv.SetData(cvframe, im.tostring(), resolution[0]*self.picMode)
        return cvframe

    def NewJpeg(self):
        frame = cv.QueryFrame( self.capture )
        jpeg = self.cvFrame2jpeg(frame,self.CAMResolution, self.picMode, self.picQuality)
        return jpeg

    def run(self):
        while not self.Die:
            
            frame = cv.QueryFrame( self.capture )
            jpeg = self.cvFrame2jpeg(frame,self.CAMResolution, self.picMode, self.picQuality)
            if self.FrameQueue.empty():
                self.FrameQueue.put(jpeg)
            #time.sleep(1/self.FPS)
            
class ClientFeedBack(threading.Thread):
        def __init__(self, connection, ErrorQueue):
            threading.Thread.__init__(self)
            self.Connection=connection
            self.ErrorQueue=ErrorQueue
            self.Die=False
            self.Queue=Queue.Queue()
            
        def run(self):
            while not self.Die:
                try:
                    data=self.Connection.recv(1024)
                except:
                    break
                if self.Queue.empty():
                    self.Queue.put(data)
            exit()
                

class FrameServer(threading.Thread):
    def __init__(self, ip, port, timeout, ErrorQueue):
        threading.Thread.__init__(self) 
        self.Ip=ip
        self.Port=port
        self.Timeout=timeout
        self.ErrorQueue=ErrorQueue
        self.Die=False
        self.Capture=Capture(self.ErrorQueue)

    def OpenSocket(self):
        self.Socket=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.Socket.bind((self.Ip, self.Port))
        self.Socket.listen(1)

    def WaitForClient(self):
        print "Warte auf Verbindung"
        try:
            self.Connection, self.ClientAddress = self.Socket.accept()
            print "Verbunden mit %s"%str(self.ClientAddress[0])
        except:
            import traceback
            self.Error.put(traceback.print_exc())
        self.ClientFeedBack=ClientFeedBack(self.Connection, self.ErrorQueue)
        self.ClientFeedBack.start()

    def ClientAction(self, data):
        if "QUALITY" in data:
            self.Capture.picQuality=int(data.split(":")[1])
            #print self.Capture.picQuality
        if "PICMODE" in data:
            self.Capture.picMode=data.split(":")[1]
            print self.Capture.picMode
        
    def ServeClient(self):
        while not self.Die:
            t=time.clock()
            try:
                self.Connection.sendall(self.Capture.NewJpeg())
            except:
                self.WaitForClient()
            
            while not self.ClientFeedBack.Queue.empty():
                self.ClientAction(self.ClientFeedBack.Queue.get())
            t1=1/(time.clock()-t)
            print t1
        
    def run(self):
        self.OpenSocket()
        self.WaitForClient()
        #self.Capture.start()
        self.ServeClient()
        
                    
class Server(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)
        self.ErrorQueue=Queue.Queue()
        #self.ControllReception=ControllReception("", 2001, 5, self.ErrorQueue)
        self.FrameServer=FrameServer("",2002,5, self.ErrorQueue)
        #self.Serial=Serial(3,9600, self.ErrorQueue)
        #self.GetErrors=GetErrors((self.Commands.Error, self.Serial.Error))
    def run(self):
        pass
        #self.ControllReception.start()
        #self.Serial.start()
        self.FrameServer.start()
        #self.GetErrors.start()
        #while True:
        #    time.sleep(1)
        #    for i in range(len(self.GetErrors.list)):
        #        self.Commands.Error.put(self.GetErrors.list[i])
        
if __name__ == "__main__":
    oServer=Server()
    oServer.start()
    #ErrorQueue=multiprocessing.Queue()
    #ControllReception=multiprocessing.Process(target=ControllReception, args=("", 2001, 5, ErrorQueue))
    #ControllReception.start()